#include "stdafx.h"

#include "collisionsolving.h"

void CollisionSolving::Collide( CircleShape *pA, CircleShape *pB )
{
    if( pA == pB )
    {
        return;
    }

    Vector2 collision = pA->mBody->mPosition - pB->mBody->mPosition;
    float distance = collision.MagnitudeSquared();

    distance -= pA->GetRadius() + pB->GetRadius();

    if( distance == 0.0f ) 
    { 
        collision = Vector2( 1.0f, 0.0f );
        distance = 1.0f;
    }
    if( distance > EPSILON )
    {
        return;
    }

    collision /= distance;
    float aci = pA->mBody->mVelocity.Dot( collision );
    float bci = pB->mBody->mVelocity.Dot( collision );

    float acf = bci;
    float bcf = aci;

    pA->mBody->mVelocity += collision * (acf - aci);
    pB->mBody->mVelocity += collision * (bcf - bci);
}
